COMMENT ⊗   VALID 00010 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	ENTRY
C00003 00003	!	cmonproc
C00009 00004	! arm interactions:  read_pos,readarm,frasg,arm_check
C00011 00005	! arm interactions:  fconstructproc
C00015 00006	!	arm motions: movepcode,alongproc,axmovproc, pbyproc,ptoproc
C00023 00007	!	drivecode,opclcode,jtmove,driveproc
C00026 00008	!	centerproc,stopproc
C00027 00009	!	opening, opclproc,closeproc
C00029 00010	!	onproc
C00034 ENDMK
C⊗;
ENTRY;
BEGIN "PPROC2"
DEFINE $$PRGID=TRUE;	

DEFINE $PPROC2=TRUE;
DEFINE $ALTER_EGO=TRUE;

REQUIRE "HEADER.SAI" SOURCE_FILE;

SIMPLE INTEGER PROCEDURE UPLEVEL(INTEGER OFFSET);
BEGIN	! eliminate this when moving to PARSE.SAI ;
	INTEGER I;
	I ← (OFFSET +1) LSH -8	; ! this gives the level ;
	I ← (I+1) LSH 8		; ! this gives the next level ;
	RETURN(I-1);
END;
!	cmonproc;
ifc false thenc
RECURSIVE PROCESURE DURCM;
	BEGIN
	RPTR(EXPR$) EXP;
	GTOKEN;
	IF TOKEN≠">"≠TOKEN≠"≥" THEN ERROR("Need > or ≥ for duration cm"
	EXP←$$GTSCEXPR("=")
endc

PROCEDURE FORCECM(rptr(expr$)e;INTEGER BITOFFSET);
	BEGIN
	INTEGER V; BOOLEAN GE; RPTR(EXPR$)EXP,ACTION;
	INTEGER I,IPC;
	INTEGER BITS,DEVBITS;
	RPTR(SYMBOL)C;
	DEVBITS←BITOFFSET LAND '7;
	WORD_READ("(");
	GTOKEN;
	IF EQU(TOKEN,"XHAT") THEN BITS←BITOFFSET
		ELSE IF EQU(TOKEN,"YHAT") THEN BITS←BITOFFSET+'1000
		ELSE IF EQU(TOKEN,"ZHAT") THEN BITS←BITOFFSET+'2000
		ELSE ERROR("FORCECM: only principal directions allowed");
	GTOKEN(")");
	GTOKEN;
	IF TOKEN="≥" OR TOKEN =">" THEN BITS←BITS+'100000
		ELSE IF TOKEN="≤" OR TOKEN="<" THEN BITS←BITS
		ELSE ERROR("FORCECM: need ≥ or < here");
	EXP←$$GTANYEXP("FORCECM",#SC);
	GTOKEN;
	IF EQU(TOKEN,"IN") THEN
		BEGIN
		GTOKEN;
		IF EQU(TOKEN,"HAND") THEN BITS←BITS
			ELSE IF EQU(TOKEN,"STATION") THEN BEGIN BITS←BITS+'400; DEVBITS←DEVBITS+'400; END
			ELSE ERROR("FORCECM: can only specify in HAND or STATION");
		WORD_READ("DO");
		END
	ELSE	BEGIN IF NOT EQU(TOKEN,"DO") THEN ERROR("FORCECM: Need DO here");
		BITS←BITS+'400; DEVBITS←DEVBITS+'400; ! default is station;
		END;
ifc false thenc	WORD_READ("STOP"); BITS←BITS+'10000; ! stop bit;
	GTOKEN;
	IF EQU(TOKEN,"BARM") THEN BEGIN DEVBITS←DEVBITS+'4; BITS←BITS+'4; END
		ELSE IF EQU(TOKEN,"YARM") THEN BEGIN DEVBITS←DEVBITS+1; BITS←BITS+1; END
		ELSE ERROR("FORCECM: can only stop an arm"); endc
	$TMPOFF←$TMPOFF+1;
	PARSE;
	ACTION←$$PCODE;
	$$PCODE←$APPEND($FRCPCODE(E,EXP,ACTION,BITS,DEVBITS),$KVARPCODE(1));
	GTOKEN(FALSE);
	END;

PROCEDURE MFORCECM(REFERENCE rptr(expr$)HEADER,HEAD,TAIL;INTEGER BITOFFSET);
	BEGIN
	INTEGER V; BOOLEAN GE; RPTR(EXPR$)EXP,ACTION;
	INTEGER I,IPC;
	INTEGER BITS,DEVBITS,TMPOFF;
	RPTR(SYMBOL)C;
!	DEVBITS←0;
	DEVBITS←BITOFFSET LAND '17;
	WORD_READ("(");
	GTOKEN;
	IF EQU(TOKEN,"XHAT") THEN BITS←BITOFFSET
		ELSE IF EQU(TOKEN,"YHAT") THEN BITS←BITOFFSET+'1000
		ELSE IF EQU(TOKEN,"ZHAT") THEN BITS←BITOFFSET+'2000
		ELSE ERROR("FORCECM: only principal directions allowed");
	WORD_READ(")");
	GTOKEN;
	IF TOKEN="≥" OR TOKEN =">" THEN BITS←BITS+'100000
		ELSE IF TOKEN="≤" OR TOKEN="<" THEN BITS←BITS
		ELSE ERROR("FORCECM: need ≥ or < here");
	EXP←$$GTANYEXP("FORCECM",#SC);
	GTOKEN;
	IF EQU(TOKEN,"IN") THEN
		BEGIN
		GTOKEN;
		IF EQU(TOKEN,"HAND") THEN BITS←BITS
			ELSE IF EQU(TOKEN,"STATION") THEN BEGIN BITS←BITS+'400; DEVBITS←DEVBITS+'400; END
			ELSE ERROR("FORCECM: can only specify in HAND or STATION");
		WORD_READ("DO");
		END
	ELSE	BEGIN IF NOT EQU(TOKEN,"DO") THEN ERROR("FORCECM: Need DO here");
		BITS←BITS+'400; DEVBITS←DEVBITS+'400; ! default is station;
		END;
	TMPOFF←$TMPOFF;	$TMPOFF←UPLEVEL($TMPOFF);
	PARSE;
	ACTION←$$PCODE;
	$TMPOFF←TMPOFF+1;
	$FFRCPCODE(HEADER,HEAD,TAIL,EXP,ACTION,BITS,DEVBITS,$TMPOFF);
	GTOKEN(FALSE);
	END;

PROCEDURE MONPROC(REFERENCE RPTR(EXPR$)HEADER,HEAD,TAIL;INTEGER BITS);
	BEGIN
	$COMPILE←$COMPILE+1;
	GTOKEN;
	IF EQU(TOKEN,"FORCE") THEN MFORCECM(HEADER,HEAD,TAIL,BITS)
	  ELSE IF EQU(TOKEN,"TORQUE") THEN MFORCECM(HEADER,HEAD,TAIL,BITS+'3000)
	  ELSE ERROR("ON: only FORCE or TORQUE available");
	$COMPILE←$COMPILE-1;
	END;

! arm interactions:  read_pos,readarm,frasg,arm_check;
IFC FALSE THENC
	! assigns the value of pos(pointer or arm) to the frame fra. If direct
	  is indicated uses it to set the rotation part;

	! returns the pointer to the input device pos (arm or pointer);

RPTR (FRAME) PROCEDURE INPT_DEV(REFERENCE STRING POS);
	BEGIN
	RPTR(FRAME) FROM;
	IF EQU(POS,"BARM")
	   THEN RETURN(F_BARM)
	   ELSE IF EQU(POS,"YARM")
		   THEN RETURN(F_YARM)
		   ELSE BEGIN
			FROM←BELONGS(POS,#FR);
			WHILE FROM≠F_BARM AND FROM≠F_YARM
			   DO	BEGIN
			        PRINT("reading on arm required");
				POS←RECOVER(POS);
				FROM←BELONGS (POS,#FR);
				END;
			RETURN(FROM);
			END;
	END;

	! reads the position of the arm from, or of the arm with pointer;

PROCEDURE READ_DEV(RPTR(FRAME) FROM);
	print("dummy call to get value of the frame");

	! reads the position of the device pos (arm or pointer);

PROCEDURE INPT(REFERENCE STRING POS);
	BEGIN
	RPTR(FRAME)FROM;
	FROM←INPT_DEV(POS);
	READ_DEV(FROM);
	END;


ENDC
! arm interactions:  fconstructproc;

	! reads an axis name and returns its number:
	  xhat=0,yhat=1,zhat=2;

IFC FALSE THENC
INTEGER PROCEDURE INPT_AXIS(REFERENCE STRING AXIS);
WHILE TRUE DO
	BEGIN
	AXIS←RECOVER(AXIS);
	IF EQU(AXIS[2 TO ∞],"HAT") THEN RETURN(AXIS - "X")
		   ELSE PRINT("--→ XHAT or YHAT or ZHAT required ←--",
				CRLF,"Try again ");
	END;
	
RPTR(TRANS) ARRAY T_CSTR[1:3]; 
		! used by CONSTRUCT instruction;

	! performs a construct instruction, without arguments;

PROCEDURE FCONSTRUCTPROC;
	BEGIN
	RPTR(FRAME) ELF;RPTR(TRANS)XFE;INTEGER I;
	RPTR(FRAME) FROM;STRING POS,ANSWER,FIRST;
	RPTR(VECTOR) V1,V2,V3;
	PRELOAD_WITH 
	    	"move arm to the origin of the frame"&CRLF,
		"move arm to the axis ",
		"move arm to the plane ";
		OWN STRING ARRAY INFORM[1:3];
	STRING AXIS;INTEGER F_AXIS,S_AXIS;

	$ALLOW←$ALLOW+1;
	GTOKEN;
	IF #TOKEN≠UNDECLARED_TYPE THEN ERROR("Need undeclared token for FCONSTRUCT")
		ELSE FIRST←TOKEN;

	AXIS←NULL;
	IF F_POINTER=NULL_RECORD
	   THEN PRINT("pointer is not defined cannot be used",CRLF)
	   ELSE POS←"POINTER";
	PRINT("three positions are required",CRLF);
	FOR I←1 STEP 1 UNTIL 3 DO
		BEGIN
	! determination of the input device required;
	   	PRINT("position ",I," read on ");
		POS←RECOVER(POS);
		FROM←INPT_DEV(POS);			! checks the input device;
	! determination of the positions for reading;
		PRINT(INFORM[I]);
		IF I=2
		   THEN F_AXIS←INPT_AXIS(AXIS)
		ELSE IF I=3
		   THEN BEGIN
			PRINT(AXIS," - ");
			AXIS←NULL;
			S_AXIS←INPT_AXIS(AXIS);
			IF S_AXIS=F_AXIS THEN ERROR("instruction not executed");
			END;
	! reading of the arm position;
		PRINT("type <cr> when the arm is at the desired position");
		ANSWER←INCHRW;
		IF ANSWER=CR 
		   THEN ANSWER←INCHRW
		   ELSE	ERROR("instruction not executed");
	 	READ_DEV(FROM);				! raads the appropriate arm pos.;
		T_CSTR[I]←ABSLOC(FROM);
		END;

	! extraction of translation part;
	V1←TPOS(T_CSTR[1]);
	V2←TPOS(T_CSTR[2]);
	V3←TPOS(T_CSTR[3]);
	
	XFE←VVVTR(V1,V2,V3,F_AXIS,S_AXIS);
	ELF←FR_INSERT(FIRST);			! inserts the new frame;
	ABSSET(ELF,XFE);			! sets the new value;
	$ALLOW←$ALLOW-1;
	IFC #DISPL THENC UPDATE;ENDC	
	END;
ENDC
!	arm motions: movepcode,alongproc,axmovproc, pbyproc,ptoproc
	moveproc, parkingproc;

	! returns the pointer to the arm affixed to obj;
RPTR(FRAME) PROCEDURE ARM_CHECK(RPTR(FRAME) OBJ);
	BEGIN
	RPTR(FRAME) TEMP;
	TEMP←OBJ;
	WHILE TEMP≠F_WRLD DO
		IF EQU(FRAME:PNAME[TEMP],"BARM") THEN RETURN(TEMP)
 		   ELSE IF EQU(FRAME:PNAME[TEMP],"YARM") THEN ERROR("YARM cannot be moved")
			ELSE TEMP←FRAME:DAD[TEMP];
	ERROR(FRAME:PNAME[OBJ]," cannot be moved");
	END;

	! saves the first part of the instruction for move commands;
PROCEDURE OLDSAV(STRING CMD,OBJ);
	BEGIN
	OLDCMD←CMD;
	OLDOBJ←OBJ;
	END;

PROCEDURE MOVEPCODE(RPTR(FRAME) MFRAME;
		 RPTR(EXPR$) ARRAY FDESTS; INTEGER NFDEST);
	BEGIN
	RPTR(SYMBOL) S1,S2; RPTR(FRAME)F1;
	S1←CHECK(FRAME:PNAME[MFRAME],#FR);
	S2←CHECK(FRAME:PNAME[F1←ARM_CHECK(MFRAME)],#FR);
	$$PCODE←$MOVEPCODE(S1,S2,FDESTS,NFDEST);
	END;


INTERNAL PROCEDURE ALONGPROC(STRING AXIS,FRA1);
	BEGIN
	INTEGER I,INDEX;
	RPTR(expr$)SCAL;RPTR(SYMBOL)SYMPTR;RPTR(FRAME)FRAM1;
	INTEGER ARRAY BUFF1[1:3],BUFF3[1:5];
	RPTR(EXPR$)ARRAY PTR[1:3],DEST[1:1];
	SCAL←$$GTANYEXP("distance to be moved along axis",#SC);
	SYMPTR←CHECK(AXIS[1 TO 1]&"HAT",#VT);
	OLDSAV("MOVE"&AXIS[1 TO 1],FRA1);  ! saves for default instructions;
	FRAM1←BELONGS(FRA1,#FR);
	INDEX←0;
 	FOR I←XAGTVAL, SYMBOL:INDEX[SYMPTR],SYMBOL:OFFSET[SYMPTR],
		XSVMUL, XTVADD  DO BUFF3[INDEX←INDEX+1]←I;
	SYMPTR←CHECK(FRA1,#FR);
	INDEX←0;
	IF SYMBOL:INDEX[SYMPTR]>0 THEN
	    FOR I←XAGTVAL, SYMBOL:INDEX[SYMPTR],SYMBOL:OFFSET[SYMPTR]
			DO BUFF1[INDEX←INDEX+1]←I
	ELSE FOR I←XGTVAL, SYMBOL:OFFSET[SYMPTR],XNOOP
			DO BUFF1[INDEX←INDEX+1]←I;
	PTR[1]←αEXPR$(BUFF1,0);
	PTR[2]←SCAL;
	PTR[3]←αEXPR$(BUFF3,0);
	DEST[1]←$AAPPEND(PTR);
	MOVEPCODE(FRAM1,DEST,1);
	END;

	! moves the frame along one axis by a scalar;

INTERNAL PROCEDURE AXMOVPROC;
	BEGIN
	STRING FRA1,AXIS; 
	AXIS←TOKEN[5 TO 5];		
	FRA1←MVFR_READ;	
	WORD_READ("BY");
	ALONGPROC(AXIS,FRA1);
	END;



	! reads/exec TO <fr>+<vt>{wrt <fr>} or BY <vector>{wrt <fr>};

INTERNAL PROCEDURE PBYPROC;
	BEGIN
 	RPTR(FRAME) FRAM1;RPTR(EXPR$)ARRAY FDEST[1:1];
				! MOVE<fr>BY<vt> ≡ MOVE<fr>TO⊗+<vt>;
		TOKEN←OLDOBJ;
		#TOKEN←ID_TYPE;
		STOKEN←TRUE;		
		$CLINR←"+"&$CLINR;
	FDEST[1]←$$GTANYEXP("destination of MOVE",#FR);
	FRAM1←BELONGS (OLDOBJ,#FR);
	MOVEPCODE(FRAM1,FDEST,1);
	END;

INTERNAL PROCEDURE PTOPROC;
	BEGIN
 	RPTR(FRAME) FRAM1; RPTR(EXPR$) ARRAY FDESTS[1:10]; INTEGER NFDEST;
	NFDEST←0;
	DO BEGIN
		FDESTS[NFDEST←NFDEST+1]←$$GTANYEXP("Destination part of MOVE",#FR);
		IF NFDEST=10 THEN ERROR("Pointy cannot currently handle more than a 9 segment move");
		GTOKEN(FALSE);
	   END UNTIL TOKEN≠",";
	STOKEN←TRUE;
	FRAM1←BELONGS (OLDOBJ,#FR);
	MOVEPCODE(FRAM1,FDESTS,NFDEST);
	END;

INTERNAL PROCEDURE MOVEPROC;
	BEGIN
	STRING FR1,AXIS;
	FR1←IDF_READ; 
	GTOKEN;
	OLDSAV("MOVE",FR1);
	IF EQU(TOKEN,"TO") THEN PTOPROC
		ELSE IF EQU(TOKEN,"BY") THEN PBYPROC
	        ELSE ERROR("TO or BY required");
	GTOKEN(FALSE);
	IF EQU(TOKEN,"ON") THEN
		BEGIN
		RPTR(EXPR$)ARRAY HEADER,HEAD,TAIL[1:15]; INTEGER #CONDS;
		RPTR(EXPR$)MOV; INTEGER BITS;
		MOV←$$PCODE;
		#CONDS←0;
		IF EQU(FR1,"BARM") THEN BITS←'4 ELSE IF
			EQU(FR1,"YARM") THEN BITS←1 ELSE
			ERROR("For force sensing can only use barm or yarm in move");
		WHILE EQU(TOKEN,"ON") DO
			MONPROC(HEADER[#CONDS←#CONDS+1],HEAD[#CONDS],TAIL[#CONDS],BITS);
			BEGIN RPTR(EXPR$)ARRAY HR,H,T[1:#CONDS];
				RPTR(EXPR$)HHR,HH,TT;
				INTEGER I;
				FOR I←1 STEP 1 UNTIL #CONDS DO
					BEGIN HR[I]←HEADER[I];
						H[I]←HEAD[I]; T[I]←TAIL[I]; END;
				HH←$APPEND($AAPPEND(H),MOV);
					EXPR$:BODY[HH][I←EXPR$:#BODY[HH]]
						←5-I;
				TT←$APPEND($AAPPEND(T),$KVARPCODE(#CONDS));
				HHR←$APPEND($AAPPEND(HR),HH);
				$$PCODE←$APPEND(HHR,TT);
			END;
		END;
	STOKEN←TRUE;
	END;

INTERNAL PROCEDURE PARKINGPROC;
	BEGIN
	STRING PAR;
	GTOKEN(FALSE);
	IF FINAL THEN ASKUSER("MOVE BARM TO BPARK; {MOVE YARM TO YPARK}")
	   ELSE IF EQU(TOKEN,"BARM") THEN ASKUSER("MOVE BARM TO BPARK")
	   ELSE IF EQU(TOKEN,"YARM") THEN ASKUSER("MOVE YARM TO YPARK")
	  ELSE ERROR("can only park BARM or YARM");
	$$PCODE←PARSE;
	END;

!	drivecode,opclcode,jtmove,driveproc;

	! drives the indicated joint of the arm (what): movement is absolute 
	  if how=to, differential if how=by;

PROCEDURE DRIVECODE(STRING WHAT,HOW;INTEGER JOINT;RPTR(EXPR$)SCAL);
	$$PCODE←$DRIVEPCODE((IF EQU(WHAT,"BJT") THEN BLUE
			ELSE YELLOW),HOW,JOINT,SCAL);

	! executes close or open instruction. How determines if the movement is 
	  absolute (to) or differential (by), op indicates the operation(open/close);

INTERNAL PROCEDURE OPCLCODE(STRING OP,HAND,HOW;RPTR(EXPR$)SCAL);
	BEGIN
	IF EQU(HAND,"BHAND")
	   THEN	IF EQU(HOW,"TO") OR EQU(OP,"OPEN")
		   THEN DRIVECODE("BJT",HOW,7,SCAL) 
		   ELSE DRIVECODE("BJT",HOW,7,$APPEND(SCAL,EXPR$1(XSNEG),#SC))
	   ELSE PRINT(#NOTYET);
	END;

	! parses the instruction
		DRIVE BJT|YJT (#) TO|BY <scalar>;

INTERNAL PROCEDURE JTMOVE(STRING WHAT,HOW;INTEGER JOINT);
	BEGIN "J"
	RPTR(EXPR$) SCAL;
  	SCAL←$$GTANYEXP("joint movement angle",#SC);
	OLDSAV("DRIVE",CVS(JOINT)); 			! saves for default instructions;
	IF EQU(WHAT,"BJT") THEN
		DRIVECODE(WHAT,HOW,JOINT,SCAL)
	ELSE PRINT(#NOTYET);
	END "J";

INTERNAL PROCEDURE DRIVEPROC;
	BEGIN
	STRING HOW;
	STRING WHAT;INTEGER JOINT;
	WHAT←IDF_READ;
	IF EQU(WHAT,"BJT") OR EQU(WHAT,"YJT")
	   THEN BEGIN
	 	WORD_READ("(");				! reads "(number)";
		GTOKEN;
		JOINT←INTSCAN(TOKEN,$BRCHR);
		IF JOINT<1 OR JOINT>7
		   THEN ERROR("non existent joint: ",cvs(joint));
		WORD_READ(")");
		HOW←IDF_READ;
		IF EQU(HOW,"BY") OR EQU(HOW,"TO")
		   THEN JTMOVE(WHAT,HOW,JOINT)
		   ELSE ERROR("TO or BY required");
		END
	   ELSE ERROR("BJT or YJT required");
	END;

!	centerproc,stopproc;

INTERNAL PROCEDURE CENTERPROC;
	BEGIN "PCENTER"
	STRING POS;
	POS←ARM_READ;		! if the arm is not indicated BARM is assumed;
	IF EQU(POS,"BARM")
	   THEN	$$PCODE←$CENTERPCODE(BLUE)
	   ELSE PRINT(#NOTYET);
	END "PCENTER";

INTERNAL PROCEDURE STOPPROC;
	BEGIN "STOPPROC"
	STRING POS;
	POS←ARM_READ;
	IF EQU(POS,"BARM")
		THEN $$PCODE←$STOPPCODE(BARM_MECH)
		ELSE PRINT(#NOTYET);
	END "STOPPROC";
!	opening, opclproc,closeproc;

INTERNAL PROCEDURE OPENING(STRING FIRST,WHAT,HOW);
	BEGIN
	RPTR(EXPR$)SCAL;
	SCAL←$$GTANYEXP("hand opening or closing",#SC);
	OLDSAV(FIRST,WHAT);			! saves for default instructions;
	OPCLCODE(FIRST,WHAT,HOW,SCAL);
	END;

	! parses the instructions
		OPEN <hand> TO|BY <scalar>;
	!	CLOSE <hand> TO|BY <scalar>;

INTERNAL PROCEDURE OPCLPROC(STRING FIRST);
	BEGIN
	STRING WHAT;
	WHAT←HAND_READ;
	GTOKEN;
	IF EQU(TOKEN,"TO") OR EQU(TOKEN,"BY")
	   THEN OPENING(FIRST,WHAT,TOKEN)
	   ELSE ERROR("Need a TO or BY for OPEN/CLOSE statement");
	END;

	! parses the instructions
	  CLOSE <hand> TO|BY <scalar> 	(BHAND as default);

INTERNAL PROCEDURE CLOSEPROC;
	BEGIN
	STRING HAND,HOW;
	GTOKEN;
	IF EQU(HAND←TOKEN,"BHAND") OR EQU(TOKEN,"YHAND") 
	    THEN GTOKEN
	    ELSE HAND←"BHAND";
	IF EQU(HOW←TOKEN,"BY") OR EQU(TOKEN,"TO")
	    THEN OPENING("CLOSE",HAND,HOW)
	    ELSE ERROR("CLOSE: need hand opening TO or BY");
	END;
!	onproc;
	
INTERNAL PROCEDURE ONPROC(RPTR(EXPR$)E(NULL_RECORD));
	BEGIN
!	IF $COMPILE=0 THEN ERROR("ON must be inside a procedure");
	$COMPILE←$COMPILE+1;
	GTOKEN;
	IF EQU(TOKEN,"FORCE") THEN FORCECM(E,0)
	  ELSE IF EQU(TOKEN,"TORQUE") THEN FORCECM(E,'3000)
	  ELSE ERROR("ON: only FORCE or TORQUE available");
	$COMPILE←$COMPILE-1;
	END;


END "PPROC2"